/*
	创建于2016/08/08
	作者：He Yujie, JLHS
*/

#include "JlRobot.h"

void JlRobot::initGyroSerial(HardwareSerial *serial)
{
  serial->begin(9600);
  this->gyroSerial = serial;
  while (serial->available())
  {
    JY901.copeSerialData(serial->read());
  }
}

void JlRobot::resetGyro()
{
  char dataSent[5] = { 0xFF, 0xAA, 0x1, lowByte(4), highByte(4) };
  this->gyroSerial->write(dataSent, 5);
}

void JlRobot::initMotor(JlMotor *lMotor, JlMotor *rMotor)
{
  this->leftMotor = lMotor;
  this->rightMotor = rMotor;
  analogWrite(lMotor->pinPwm, 0);
  analogWrite(rMotor->pinPwm, 0);
  pinMode(lMotor->pinA, OUTPUT);
  pinMode(lMotor->pinB, OUTPUT);
  pinMode(rMotor->pinA, OUTPUT);
  pinMode(rMotor->pinB, OUTPUT);
  digitalWrite(lMotor->pinA, LOW);
  digitalWrite(lMotor->pinB, LOW);
  digitalWrite(rMotor->pinA, LOW);
  digitalWrite(rMotor->pinB, LOW);
}

void JlRobot::initExtServo(HardwareSerial *serial)
{
  serial->begin(9600);
  this->servoSerial = serial;
}

void JlRobot::setMotor(JlMotor &motor, int speed)
{
  if (speed > 90) { this->setMotor(motor, 90); return; }
  if (speed < -90) { this->setMotor(motor, -90); return; }
  pinMode(motor.pinA, OUTPUT);
  pinMode(motor.pinB, OUTPUT);
  int oa = digitalRead(motor.pinA);
  int ob = digitalRead(motor.pinB);
  if (oa == LOW && ob == LOW)
  {
    if (speed > 0)
    {
      digitalWrite(motor.pinA, HIGH);
      analogWrite(motor.pinPwm, speed / 100.0 * 255); 
    }
    else if (speed < 0)
    {
      digitalWrite(motor.pinB, HIGH);
      analogWrite(motor.pinPwm, -speed / 100.0 * 255);
    }
  }
  else if (oa == HIGH && ob == LOW)
  {
    if (speed > 0)
    {
      analogWrite(motor.pinPwm, speed / 100.0 * 255);
    }
    else if (speed == 0)
    {
      digitalWrite(motor.pinA, LOW);
      analogWrite(motor.pinPwm, 0);
    }
    else if (speed < 0)
    {
      analogWrite(motor.pinPwm, -speed / 100.0 * 255);
      digitalWrite(motor.pinA, LOW);
      digitalWrite(motor.pinB, HIGH);
    }
  }
  else if (oa == LOW && ob == HIGH)
  {
    if (speed < 0)
    {
      analogWrite(motor.pinPwm, -speed / 100.0 * 255);
    }
    else if (speed == 0)
    {
      digitalWrite(motor.pinB, LOW);
      analogWrite(motor.pinPwm, 0);
    }
    else if (speed > 0)
    {
      analogWrite(motor.pinPwm, speed / 100.0 * 255);
      digitalWrite(motor.pinB, LOW);
      digitalWrite(motor.pinA, HIGH);
    }
  }
}

void JlRobot::run(int lSpeed, int rSpeed)
{
  if (this->leftMotor != 0 && this->rightMotor != 0)
  {
    this->setMotor(*this->leftMotor, lSpeed);
    this->setMotor(*this->rightMotor, rSpeed);
  }
}

void JlRobot::setExtServo(char channel, int angle, int speed)
{
  if (speed < 0 || speed > 20 || angle < 0 || angle > 180)
  {
    return;
  }
  angle = map(angle, 0, 180, 500, 2500);
  char dataSent[5] = { 0xFF, 0x01, channel, speed & 0xFF, speed >> 8 }; 
  this->servoSerial->write(dataSent, 5);
  char dataSent2[5] = { 0xFF, 0x02, channel, angle & 0xFF, angle >> 8 };
  this->servoSerial->write(dataSent2, 5);
}

void JlRobot::actExtServo(int number)
{
  if (number < 0 || number > 15)
  {
    return;
  }
  char dataSent[5] = { 0xFF,0x09, 0x0, number & 0xFF, number >> 8 };
  this->servoSerial->write(dataSent, 5);
}

bool JlRobot::setServo(Servo &servo, unsigned int angle)
{
	if (&servo == NULL)
	{
		return false;
	}
	else
	{
		if (angle > 180) { angle = 180; }
		servo.write(angle);
		return true;
	}
}

bool JlRobot::setServo(Servo &servo, int angle, int speed)
{
	if (&servo == NULL)
	{
		return false;
	}
	if (angle > 180)
	{
		angle = 180;
	}
	if (speed > 100)
	{
		speed = 100;
	}
	int tspeed = 101 - speed;
	int oangle = servo.read();
	float oneTimeDiff = float(angle - oangle) / float(tspeed);
	for (int i = 0;i < tspeed;i++)
	{
		//Serial.println(oneTimeDiff);
		servo.write(oangle + int(oneTimeDiff * double(i + 1)));
		delay(20);
	}
	//servo->write(angle);
	return true;
}
